#include "precompiled.h"
#pragma hdrstop


namespace inf {
    //===============================================================
    //
    //	Mat2
    //
    //===============================================================

    Mat2 Mat2::ZERO( Vec2( 0, 0 ), Vec2( 0, 0 ) );
    Mat2 Mat2::IDENTITY( Vec2( 1, 0 ), Vec2( 0, 1 ) );

    /*
    ============
    Mat2::inverse_self
    ============
    */
    bool Mat2::inverse_self( void ) {
        // 2+4 = 6 multiplications
        //		 1 division
        double det, invDet, a;

        det = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];

        if ( math::fabs( det ) < MATRIX_INVERSE_EPSILON ) {
            return false;
        }

        invDet = 1.0f / det;

        a = mat[0][0];
        mat[0][0] =   mat[1][1] * invDet;
        mat[0][1] = - mat[0][1] * invDet;
        mat[1][0] = - mat[1][0] * invDet;
        mat[1][1] =   a * invDet;

        return true;
    }

    /*
    ============
    Mat2::inverse_fast_self
    ============
    */
    bool Mat2::inverse_fast_self( void ) {
#if 1
        // 2+4 = 6 multiplications
        //		 1 division
        double det, invDet, a;

        det = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];

        if ( math::fabs( det ) < MATRIX_INVERSE_EPSILON ) {
            return false;
        }

        invDet = 1.0f / det;

        a = mat[0][0];
        mat[0][0] =   mat[1][1] * invDet;
        mat[0][1] = - mat[0][1] * invDet;
        mat[1][0] = - mat[1][0] * invDet;
        mat[1][1] =   a * invDet;

        return true;
#else
        // 2*4 = 8 multiplications
        //		 2 division
        float *mat = reinterpret_cast<float *>(this);
        double d, di;
        float s;

        di = mat[0];
        s = di;
        mat[0*2+0] = d = 1.0f / di;
        mat[0*2+1] *= d;
        d = -d;
        mat[1*2+0] *= d;
        d = mat[1*2+0] * di;
        mat[1*2+1] += mat[0*2+1] * d;
        di = mat[1*2+1];
        s *= di;
        mat[1*2+1] = d = 1.0f / di;
        mat[1*2+0] *= d;
        d = -d;
        mat[0*2+1] *= d;
        d = mat[0*2+1] * di;
        mat[0*2+0] += mat[1*2+0] * d;

        return ( s != 0.0f && !FLOAT_IS_NAN( s ) );
#endif
    }

    /*
    =============
    Mat2::to_string
    =============
    */
    const char *Mat2::to_string( int precision ) const {
        return float_array_to_string_ANSI( to_float_ptr(), get_dimension(), precision );
    }


    //===============================================================
    //
    //	Mat3
    //
    //===============================================================

    Mat3 Mat3::ZERO( Vec3( 0, 0, 0 ), Vec3( 0, 0, 0 ), Vec3( 0, 0, 0 ) );
    Mat3 Mat3::IDENTITY( Vec3( 1, 0, 0 ), Vec3( 0, 1, 0 ), Vec3( 0, 0, 1 ) );

    /*
    ============
    Mat3::to_Angles
    ============
    */
    Angles Mat3::to_Angles( void ) const {
        Angles	angles;
        double		theta;
        double		cp;
        float		sp;

        sp = mat[ 0 ][ 2 ];

        // cap off our sin value so that we don't get any NANs
        if ( sp > 1.0f ) {
            sp = 1.0f;
        } else if ( sp < -1.0f ) {
            sp = -1.0f;
        }

        theta = -math::asin( sp );
        cp = math::cos( theta );

        if ( cp > 8192.0f * math::FLOAT_EPSILON ) {
            angles.pitch	= math::RAD2DEG( theta );
            angles.yaw		= math::RAD2DEG( math::atan( mat[ 0 ][ 1 ], mat[ 0 ][ 0 ] ) );
            angles.roll		= math::RAD2DEG( math::atan( mat[ 1 ][ 2 ], mat[ 2 ][ 2 ] ) );
        } else {
            angles.pitch	= math::RAD2DEG( theta );
            angles.yaw		= math::RAD2DEG( -math::atan( mat[ 1 ][ 0 ], mat[ 1 ][ 1 ] ) );
            angles.roll		= 0;
        }
        return angles;
    }

    /*
    ============
    Mat3::to_Quat
    ============
    */
    Quat Mat3::to_Quat( void ) const {
        Quat		q;
        float		trace;
        float		s;
        float		t;
        int     	i;
        int			j;
        int			k;

        static int 	next[ 3 ] = { 1, 2, 0 };

        trace = mat[ 0 ][ 0 ] + mat[ 1 ][ 1 ] + mat[ 2 ][ 2 ];

        if ( trace > 0.0f ) {

            t = trace + 1.0f;
            s = math::inv_sqrt( t ) * 0.5f;

            q[3] = s * t;
            q[0] = ( mat[ 2 ][ 1 ] - mat[ 1 ][ 2 ] ) * s;
            q[1] = ( mat[ 0 ][ 2 ] - mat[ 2 ][ 0 ] ) * s;
            q[2] = ( mat[ 1 ][ 0 ] - mat[ 0 ][ 1 ] ) * s;

        } else {

            i = 0;
            if ( mat[ 1 ][ 1 ] > mat[ 0 ][ 0 ] ) {
                i = 1;
            }
            if ( mat[ 2 ][ 2 ] > mat[ i ][ i ] ) {
                i = 2;
            }
            j = next[ i ];
            k = next[ j ];

            t = ( mat[ i ][ i ] - ( mat[ j ][ j ] + mat[ k ][ k ] ) ) + 1.0f;
            s = math::inv_sqrt( t ) * 0.5f;

            q[i] = s * t;
            q[3] = ( mat[ k ][ j ] - mat[ j ][ k ] ) * s;
            q[j] = ( mat[ j ][ i ] + mat[ i ][ j ] ) * s;
            q[k] = ( mat[ k ][ i ] + mat[ i ][ k ] ) * s;
        }
        return q;
    }

    /*
    ============
    Mat3::to_CQuat
    ============
    */
    CQuat Mat3::to_CQuat( void ) const {
        Quat q = to_Quat();
        if ( q.w < 0.0f ) {
            return CQuat( -q.x, -q.y, -q.z );
        }
        return CQuat( q.x, q.y, q.z );
    }

    /*
    ============
    Mat3::to_Rotation
    ============
    */
    Rotation Mat3::to_Rotation( void ) const {
        Rotation	r;
        float		trace;
        float		s;
        float		t;
        int     	i;
        int			j;
        int			k;
        static int 	next[ 3 ] = { 1, 2, 0 };

        trace = mat[ 0 ][ 0 ] + mat[ 1 ][ 1 ] + mat[ 2 ][ 2 ];
        if ( trace > 0.0f ) {

            t = trace + 1.0f;
            s = math::inv_sqrt( t ) * 0.5f;

            r.angle = s * t;
            r.vec[0] = ( mat[ 2 ][ 1 ] - mat[ 1 ][ 2 ] ) * s;
            r.vec[1] = ( mat[ 0 ][ 2 ] - mat[ 2 ][ 0 ] ) * s;
            r.vec[2] = ( mat[ 1 ][ 0 ] - mat[ 0 ][ 1 ] ) * s;

        } else {

            i = 0;
            if ( mat[ 1 ][ 1 ] > mat[ 0 ][ 0 ] ) {
                i = 1;
            }
            if ( mat[ 2 ][ 2 ] > mat[ i ][ i ] ) {
                i = 2;
            }
            j = next[ i ];  
            k = next[ j ];

            t = ( mat[ i ][ i ] - ( mat[ j ][ j ] + mat[ k ][ k ] ) ) + 1.0f;
            s = math::inv_sqrt( t ) * 0.5f;

            r.vec[i]	= s * t;
            r.angle		= ( mat[ k ][ j ] - mat[ j ][ k ] ) * s;
            r.vec[j]	= ( mat[ j ][ i ] + mat[ i ][ j ] ) * s;
            r.vec[k]	= ( mat[ k ][ i ] + mat[ i ][ k ] ) * s;
        }
        r.angle = math::acos( r.angle );
        if ( ( math::fabs( r.angle ) < 1e-10f ) || !( r.vec.normalize() > 0.0f ) ) {
            r.vec.set( 0.0f, 0.0f, 1.0f );
            r.angle = 0.0f;
        } else {
            //vec *= (1.0f / sin( angle ));
            r.vec.fix_degenerate_normal();
            r.angle *= 2.0f * math::M_RAD2DEG;
        }

        r.origin.zero();
        r.axis = *this;
        r.axisValid = true;
        return r;
    }

    /*
    =================
    Mat3::to_angular_velocity
    =================
    */
    Vec3 Mat3::to_angular_velocity( void ) const {
        Rotation rotation = to_Rotation();
        return rotation.get_Vec() * math::DEG2RAD( rotation.get_angle() );
    }

    /*
    ============
    Mat3::determinant
    ============
    */
    float Mat3::determinant( void ) const {

        float det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
        float det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0];
        float det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];

        return mat[0][0] * det2_12_12 - mat[0][1] * det2_12_02 + mat[0][2] * det2_12_01;
    }

    /*
    ============
    Mat3::inverse_self
    ============
    */
    bool Mat3::inverse_self( void ) {
        // 18+3+9 = 30 multiplications
        //			 1 division
        Mat3 inverse;
        double det, invDet;

        inverse[0][0] = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
        inverse[1][0] = mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2];
        inverse[2][0] = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];

        det = mat[0][0] * inverse[0][0] + mat[0][1] * inverse[1][0] + mat[0][2] * inverse[2][0];

        if ( math::fabs( det ) < MATRIX_INVERSE_EPSILON ) {
            return false;
        }

        invDet = 1.0f / det;

        inverse[0][1] = mat[0][2] * mat[2][1] - mat[0][1] * mat[2][2];
        inverse[0][2] = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
        inverse[1][1] = mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0];
        inverse[1][2] = mat[0][2] * mat[1][0] - mat[0][0] * mat[1][2];
        inverse[2][1] = mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1];
        inverse[2][2] = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];

        mat[0][0] = inverse[0][0] * invDet;
        mat[0][1] = inverse[0][1] * invDet;
        mat[0][2] = inverse[0][2] * invDet;

        mat[1][0] = inverse[1][0] * invDet;
        mat[1][1] = inverse[1][1] * invDet;
        mat[1][2] = inverse[1][2] * invDet;

        mat[2][0] = inverse[2][0] * invDet;
        mat[2][1] = inverse[2][1] * invDet;
        mat[2][2] = inverse[2][2] * invDet;

        return true;
    }

    /*
    ============
    Mat3::inverse_fast_self
    ============
    */
    bool Mat3::inverse_fast_self( void ) {
#if 1
        // 18+3+9 = 30 multiplications
        //			 1 division
        Mat3 inverse;
        double det, invDet;

        inverse[0][0] = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
        inverse[1][0] = mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2];
        inverse[2][0] = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];

        det = mat[0][0] * inverse[0][0] + mat[0][1] * inverse[1][0] + mat[0][2] * inverse[2][0];

        if ( math::fabs( det ) < MATRIX_INVERSE_EPSILON ) {
            return false;
        }

        invDet = 1.0f / det;

        inverse[0][1] = mat[0][2] * mat[2][1] - mat[0][1] * mat[2][2];
        inverse[0][2] = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
        inverse[1][1] = mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0];
        inverse[1][2] = mat[0][2] * mat[1][0] - mat[0][0] * mat[1][2];
        inverse[2][1] = mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1];
        inverse[2][2] = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];

        mat[0][0] = inverse[0][0] * invDet;
        mat[0][1] = inverse[0][1] * invDet;
        mat[0][2] = inverse[0][2] * invDet;

        mat[1][0] = inverse[1][0] * invDet;
        mat[1][1] = inverse[1][1] * invDet;
        mat[1][2] = inverse[1][2] * invDet;

        mat[2][0] = inverse[2][0] * invDet;
        mat[2][1] = inverse[2][1] * invDet;
        mat[2][2] = inverse[2][2] * invDet;

        return true;
#elif 0
        // 3*10 = 30 multiplications
        //		   3 divisions
        float *mat = reinterpret_cast<float *>(this);
        float s;
        double d, di;

        di = mat[0];
        s = di;
        mat[0] = d = 1.0f / di;
        mat[1] *= d;
        mat[2] *= d;
        d = -d;
        mat[3] *= d;
        mat[6] *= d;
        d = mat[3] * di;
        mat[4] += mat[1] * d;
        mat[5] += mat[2] * d;
        d = mat[6] * di;
        mat[7] += mat[1] * d;
        mat[8] += mat[2] * d;
        di = mat[4];
        s *= di;
        mat[4] = d = 1.0f / di;
        mat[3] *= d;
        mat[5] *= d;
        d = -d;
        mat[1] *= d;
        mat[7] *= d;
        d = mat[1] * di;
        mat[0] += mat[3] * d;
        mat[2] += mat[5] * d;
        d = mat[7] * di;
        mat[6] += mat[3] * d;
        mat[8] += mat[5] * d;
        di = mat[8];
        s *= di;
        mat[8] = d = 1.0f / di;
        mat[6] *= d;
        mat[7] *= d;
        d = -d;
        mat[2] *= d;
        mat[5] *= d;
        d = mat[2] * di;
        mat[0] += mat[6] * d;
        mat[1] += mat[7] * d;
        d = mat[5] * di;
        mat[3] += mat[6] * d;
        mat[4] += mat[7] * d;

        return ( s != 0.0f && !FLOAT_IS_NAN( s ) );
#else
        //	4*2+4*4 = 24 multiplications
        //		2*1 =  2 divisions
        Mat2 r0;
        float r1[2], r2[2], r3;
        float det, invDet;
        float *mat = reinterpret_cast<float *>(this);

        // r0 = m0.inverse();	// 2x2
        det = mat[0*3+0] * mat[1*3+1] - mat[0*3+1] * mat[1*3+0];

        if ( math::fabs( det ) < MATRIX_INVERSE_EPSILON ) {
            return false;
        }

        invDet = 1.0f / det;

        r0[0][0] =   mat[1*3+1] * invDet;
        r0[0][1] = - mat[0*3+1] * invDet;
        r0[1][0] = - mat[1*3+0] * invDet;
        r0[1][1] =   mat[0*3+0] * invDet;

        // r1 = r0 * m1;		// 2x1 = 2x2 * 2x1
        r1[0] = r0[0][0] * mat[0*3+2] + r0[0][1] * mat[1*3+2];
        r1[1] = r0[1][0] * mat[0*3+2] + r0[1][1] * mat[1*3+2];

        // r2 = m2 * r1;		// 1x1 = 1x2 * 2x1
        r2[0] = mat[2*3+0] * r1[0] + mat[2*3+1] * r1[1];

        // r3 = r2 - m3;		// 1x1 = 1x1 - 1x1
        r3 = r2[0] - mat[2*3+2];

        // r3.inverse_self();
        if ( math::fabs( r3 ) < MATRIX_INVERSE_EPSILON ) {
            return false;
        }

        r3 = 1.0f / r3;

        // r2 = m2 * r0;		// 1x2 = 1x2 * 2x2
        r2[0] = mat[2*3+0] * r0[0][0] + mat[2*3+1] * r0[1][0];
        r2[1] = mat[2*3+0] * r0[0][1] + mat[2*3+1] * r0[1][1];

        // m2 = r3 * r2;		// 1x2 = 1x1 * 1x2
        mat[2*3+0] = r3 * r2[0];
        mat[2*3+1] = r3 * r2[1];

        // m0 = r0 - r1 * m2;	// 2x2 - 2x1 * 1x2
        mat[0*3+0] = r0[0][0] - r1[0] * mat[2*3+0];
        mat[0*3+1] = r0[0][1] - r1[0] * mat[2*3+1];
        mat[1*3+0] = r0[1][0] - r1[1] * mat[2*3+0];
        mat[1*3+1] = r0[1][1] - r1[1] * mat[2*3+1];

        // m1 = r1 * r3;		// 2x1 = 2x1 * 1x1
        mat[0*3+2] = r1[0] * r3;
        mat[1*3+2] = r1[1] * r3;

        // m3 = -r3;
        mat[2*3+2] = -r3;

        return true;
#endif
    }

    /*
    ============
    Mat3::inertia_translate
    ============
    */
    Mat3 Mat3::inertia_translate( const float mass, const Vec3 &centerOfMass, const Vec3 &translation ) const {
        Mat3 m;
        Vec3 newCenter;

        newCenter = centerOfMass + translation;

        m[0][0] = mass * ( ( centerOfMass[1] * centerOfMass[1] + centerOfMass[2] * centerOfMass[2] )
            - ( newCenter[1] * newCenter[1] + newCenter[2] * newCenter[2] ) );
        m[1][1] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[2] * centerOfMass[2] )
            - ( newCenter[0] * newCenter[0] + newCenter[2] * newCenter[2] ) );
        m[2][2] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[1] * centerOfMass[1] )
            - ( newCenter[0] * newCenter[0] + newCenter[1] * newCenter[1] ) );

        m[0][1] = m[1][0] = mass * ( newCenter[0] * newCenter[1] - centerOfMass[0] * centerOfMass[1] );
        m[1][2] = m[2][1] = mass * ( newCenter[1] * newCenter[2] - centerOfMass[1] * centerOfMass[2] );
        m[0][2] = m[2][0] = mass * ( newCenter[0] * newCenter[2] - centerOfMass[0] * centerOfMass[2] );

        return (*this) + m;
    }

    /*
    ============
    Mat3::inertia_translate_self
    ============
    */
    Mat3 &Mat3::inertia_translate_self( const float mass, const Vec3 &centerOfMass, const Vec3 &translation ) {
        Mat3 m;
        Vec3 newCenter;

        newCenter = centerOfMass + translation;

        m[0][0] = mass * ( ( centerOfMass[1] * centerOfMass[1] + centerOfMass[2] * centerOfMass[2] )
            - ( newCenter[1] * newCenter[1] + newCenter[2] * newCenter[2] ) );
        m[1][1] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[2] * centerOfMass[2] )
            - ( newCenter[0] * newCenter[0] + newCenter[2] * newCenter[2] ) );
        m[2][2] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[1] * centerOfMass[1] )
            - ( newCenter[0] * newCenter[0] + newCenter[1] * newCenter[1] ) );

        m[0][1] = m[1][0] = mass * ( newCenter[0] * newCenter[1] - centerOfMass[0] * centerOfMass[1] );
        m[1][2] = m[2][1] = mass * ( newCenter[1] * newCenter[2] - centerOfMass[1] * centerOfMass[2] );
        m[0][2] = m[2][0] = mass * ( newCenter[0] * newCenter[2] - centerOfMass[0] * centerOfMass[2] );

        (*this) += m;

        return (*this);
    }

    /*
    ============
    Mat3::inertia_rotate
    ============
    */
    Mat3 Mat3::inertia_rotate( const Mat3 &rotation ) const {
        // NOTE: the rotation matrix is stored column-major
        return rotation.transpose() * (*this) * rotation;
    }

    /*
    ============
    Mat3::inertia_rotate_self
    ============
    */
    Mat3 &Mat3::inertia_rotate_self( const Mat3 &rotation ) {
        // NOTE: the rotation matrix is stored column-major
        *this = rotation.transpose() * (*this) * rotation;
        return *this;
    }

    /*
    =============
    Mat3::to_string
    =============
    */
    const char *Mat3::to_string( int precision ) const {
        return float_array_to_string_ANSI( to_float_ptr(), get_dimension(), precision );
    }


    //===============================================================
    //
    //	Mat4
    //
    //===============================================================

    Mat4 Mat4::ZERO( Vec4( 0, 0, 0, 0 ), Vec4( 0, 0, 0, 0 ), Vec4( 0, 0, 0, 0 ), Vec4( 0, 0, 0, 0 ) );
    Mat4 Mat4::IDENTITY( Vec4( 1, 0, 0, 0 ), Vec4( 0, 1, 0, 0 ), Vec4( 0, 0, 1, 0 ), Vec4( 0, 0, 0, 1 ) );

    /*
    ============
    Mat4::transpose
    ============
    */
    Mat4 Mat4::transpose( void ) const {
        Mat4	transpose;
        int		i, j;

        for( i = 0; i < 4; i++ ) {
            for( j = 0; j < 4; j++ ) {
                transpose[ i ][ j ] = mat[ j ][ i ];
            }
        }
        return transpose;
    }

    /*
    ============
    Mat4::transpose_self
    ============
    */
    Mat4 &Mat4::transpose_self( void ) {
        float	temp;
        int		i, j;

        for( i = 0; i < 4; i++ ) {
            for( j = i + 1; j < 4; j++ ) {
                temp = mat[ i ][ j ];
                mat[ i ][ j ] = mat[ j ][ i ];
                mat[ j ][ i ] = temp;
            }
        }
        return *this;
    }

    /*
    ============
    Mat4::determinant
    ============
    */
    float Mat4::determinant( void ) const {

        // 2x2 sub-determinants
        float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
        float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
        float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
        float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
        float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
        float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];

        // 3x3 sub-determinants
        float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
        float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
        float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
        float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;

        return ( - det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3] );
    }

    /*
    ============
    Mat4::inverse_self
    ============
    */
    bool Mat4::inverse_self( void ) {
        // 84+4+16 = 104 multiplications
        //			   1 division
        double det, invDet;

        // 2x2 sub-determinants required to calculate 4x4 determinant
        float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
        float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
        float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
        float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
        float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
        float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];

        // 3x3 sub-determinants required to calculate 4x4 determinant
        float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
        float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
        float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
        float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;

        det = ( - det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3] );

        if ( math::fabs( det ) < MATRIX_INVERSE_EPSILON ) {
            return false;
        }

        invDet = 1.0f / det;

        // remaining 2x2 sub-determinants
        float det2_03_01 = mat[0][0] * mat[3][1] - mat[0][1] * mat[3][0];
        float det2_03_02 = mat[0][0] * mat[3][2] - mat[0][2] * mat[3][0];
        float det2_03_03 = mat[0][0] * mat[3][3] - mat[0][3] * mat[3][0];
        float det2_03_12 = mat[0][1] * mat[3][2] - mat[0][2] * mat[3][1];
        float det2_03_13 = mat[0][1] * mat[3][3] - mat[0][3] * mat[3][1];
        float det2_03_23 = mat[0][2] * mat[3][3] - mat[0][3] * mat[3][2];

        float det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];
        float det2_13_02 = mat[1][0] * mat[3][2] - mat[1][2] * mat[3][0];
        float det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];
        float det2_13_12 = mat[1][1] * mat[3][2] - mat[1][2] * mat[3][1];
        float det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];
        float det2_13_23 = mat[1][2] * mat[3][3] - mat[1][3] * mat[3][2];

        // remaining 3x3 sub-determinants
        float det3_203_012 = mat[2][0] * det2_03_12 - mat[2][1] * det2_03_02 + mat[2][2] * det2_03_01;
        float det3_203_013 = mat[2][0] * det2_03_13 - mat[2][1] * det2_03_03 + mat[2][3] * det2_03_01;
        float det3_203_023 = mat[2][0] * det2_03_23 - mat[2][2] * det2_03_03 + mat[2][3] * det2_03_02;
        float det3_203_123 = mat[2][1] * det2_03_23 - mat[2][2] * det2_03_13 + mat[2][3] * det2_03_12;

        float det3_213_012 = mat[2][0] * det2_13_12 - mat[2][1] * det2_13_02 + mat[2][2] * det2_13_01;
        float det3_213_013 = mat[2][0] * det2_13_13 - mat[2][1] * det2_13_03 + mat[2][3] * det2_13_01;
        float det3_213_023 = mat[2][0] * det2_13_23 - mat[2][2] * det2_13_03 + mat[2][3] * det2_13_02;
        float det3_213_123 = mat[2][1] * det2_13_23 - mat[2][2] * det2_13_13 + mat[2][3] * det2_13_12;

        float det3_301_012 = mat[3][0] * det2_01_12 - mat[3][1] * det2_01_02 + mat[3][2] * det2_01_01;
        float det3_301_013 = mat[3][0] * det2_01_13 - mat[3][1] * det2_01_03 + mat[3][3] * det2_01_01;
        float det3_301_023 = mat[3][0] * det2_01_23 - mat[3][2] * det2_01_03 + mat[3][3] * det2_01_02;
        float det3_301_123 = mat[3][1] * det2_01_23 - mat[3][2] * det2_01_13 + mat[3][3] * det2_01_12;

        mat[0][0] =	- det3_213_123 * invDet;
        mat[1][0] = + det3_213_023 * invDet;
        mat[2][0] = - det3_213_013 * invDet;
        mat[3][0] = + det3_213_012 * invDet;

        mat[0][1] = + det3_203_123 * invDet;
        mat[1][1] = - det3_203_023 * invDet;
        mat[2][1] = + det3_203_013 * invDet;
        mat[3][1] = - det3_203_012 * invDet;

        mat[0][2] = + det3_301_123 * invDet;
        mat[1][2] = - det3_301_023 * invDet;
        mat[2][2] = + det3_301_013 * invDet;
        mat[3][2] = - det3_301_012 * invDet;

        mat[0][3] = - det3_201_123 * invDet;
        mat[1][3] = + det3_201_023 * invDet;
        mat[2][3] = - det3_201_013 * invDet;
        mat[3][3] = + det3_201_012 * invDet;

        return true;
    }

    /*
    ============
    Mat4::inverse_fast_self
    ============
    */
    bool Mat4::inverse_fast_self( void ) {
#if 0
        // 84+4+16 = 104 multiplications
        //			   1 division
        double det, invDet;

        // 2x2 sub-determinants required to calculate 4x4 determinant
        float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
        float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
        float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
        float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
        float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
        float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];

        // 3x3 sub-determinants required to calculate 4x4 determinant
        float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
        float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
        float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
        float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;

        det = ( - det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3] );

        if ( math::fabs( det ) < MATRIX_INVERSE_EPSILON ) {
            return false;
        }

        invDet = 1.0f / det;

        // remaining 2x2 sub-determinants
        float det2_03_01 = mat[0][0] * mat[3][1] - mat[0][1] * mat[3][0];
        float det2_03_02 = mat[0][0] * mat[3][2] - mat[0][2] * mat[3][0];
        float det2_03_03 = mat[0][0] * mat[3][3] - mat[0][3] * mat[3][0];
        float det2_03_12 = mat[0][1] * mat[3][2] - mat[0][2] * mat[3][1];
        float det2_03_13 = mat[0][1] * mat[3][3] - mat[0][3] * mat[3][1];
        float det2_03_23 = mat[0][2] * mat[3][3] - mat[0][3] * mat[3][2];

        float det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];
        float det2_13_02 = mat[1][0] * mat[3][2] - mat[1][2] * mat[3][0];
        float det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];
        float det2_13_12 = mat[1][1] * mat[3][2] - mat[1][2] * mat[3][1];
        float det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];
        float det2_13_23 = mat[1][2] * mat[3][3] - mat[1][3] * mat[3][2];

        // remaining 3x3 sub-determinants
        float det3_203_012 = mat[2][0] * det2_03_12 - mat[2][1] * det2_03_02 + mat[2][2] * det2_03_01;
        float det3_203_013 = mat[2][0] * det2_03_13 - mat[2][1] * det2_03_03 + mat[2][3] * det2_03_01;
        float det3_203_023 = mat[2][0] * det2_03_23 - mat[2][2] * det2_03_03 + mat[2][3] * det2_03_02;
        float det3_203_123 = mat[2][1] * det2_03_23 - mat[2][2] * det2_03_13 + mat[2][3] * det2_03_12;

        float det3_213_012 = mat[2][0] * det2_13_12 - mat[2][1] * det2_13_02 + mat[2][2] * det2_13_01;
        float det3_213_013 = mat[2][0] * det2_13_13 - mat[2][1] * det2_13_03 + mat[2][3] * det2_13_01;
        float det3_213_023 = mat[2][0] * det2_13_23 - mat[2][2] * det2_13_03 + mat[2][3] * det2_13_02;
        float det3_213_123 = mat[2][1] * det2_13_23 - mat[2][2] * det2_13_13 + mat[2][3] * det2_13_12;

        float det3_301_012 = mat[3][0] * det2_01_12 - mat[3][1] * det2_01_02 + mat[3][2] * det2_01_01;
        float det3_301_013 = mat[3][0] * det2_01_13 - mat[3][1] * det2_01_03 + mat[3][3] * det2_01_01;
        float det3_301_023 = mat[3][0] * det2_01_23 - mat[3][2] * det2_01_03 + mat[3][3] * det2_01_02;
        float det3_301_123 = mat[3][1] * det2_01_23 - mat[3][2] * det2_01_13 + mat[3][3] * det2_01_12;

        mat[0][0] =	- det3_213_123 * invDet;
        mat[1][0] = + det3_213_023 * invDet;
        mat[2][0] = - det3_213_013 * invDet;
        mat[3][0] = + det3_213_012 * invDet;

        mat[0][1] = + det3_203_123 * invDet;
        mat[1][1] = - det3_203_023 * invDet;
        mat[2][1] = + det3_203_013 * invDet;
        mat[3][1] = - det3_203_012 * invDet;

        mat[0][2] = + det3_301_123 * invDet;
        mat[1][2] = - det3_301_023 * invDet;
        mat[2][2] = + det3_301_013 * invDet;
        mat[3][2] = - det3_301_012 * invDet;

        mat[0][3] = - det3_201_123 * invDet;
        mat[1][3] = + det3_201_023 * invDet;
        mat[2][3] = - det3_201_013 * invDet;
        mat[3][3] = + det3_201_012 * invDet;

        return true;
#elif 0
        // 4*18 = 72 multiplications
        //		   4 divisions
        float *mat = reinterpret_cast<float *>(this);
        float s;
        double d, di;

        di = mat[0];
        s = di;
        mat[0] = d = 1.0f / di;
        mat[1] *= d;
        mat[2] *= d;
        mat[3] *= d;
        d = -d;
        mat[4] *= d;
        mat[8] *= d;
        mat[12] *= d;
        d = mat[4] * di;
        mat[5] += mat[1] * d;
        mat[6] += mat[2] * d;
        mat[7] += mat[3] * d;
        d = mat[8] * di;
        mat[9] += mat[1] * d;
        mat[10] += mat[2] * d;
        mat[11] += mat[3] * d;
        d = mat[12] * di;
        mat[13] += mat[1] * d;
        mat[14] += mat[2] * d;
        mat[15] += mat[3] * d;
        di = mat[5];
        s *= di;
        mat[5] = d = 1.0f / di;
        mat[4] *= d;
        mat[6] *= d;
        mat[7] *= d;
        d = -d;
        mat[1] *= d;
        mat[9] *= d;
        mat[13] *= d;
        d = mat[1] * di;
        mat[0] += mat[4] * d;
        mat[2] += mat[6] * d;
        mat[3] += mat[7] * d;
        d = mat[9] * di;
        mat[8] += mat[4] * d;
        mat[10] += mat[6] * d;
        mat[11] += mat[7] * d;
        d = mat[13] * di;
        mat[12] += mat[4] * d;
        mat[14] += mat[6] * d;
        mat[15] += mat[7] * d;
        di = mat[10];
        s *= di;
        mat[10] = d = 1.0f / di;
        mat[8] *= d;
        mat[9] *= d;
        mat[11] *= d;
        d = -d;
        mat[2] *= d;
        mat[6] *= d;
        mat[14] *= d;
        d = mat[2] * di;
        mat[0] += mat[8] * d;
        mat[1] += mat[9] * d;
        mat[3] += mat[11] * d;
        d = mat[6] * di;
        mat[4] += mat[8] * d;
        mat[5] += mat[9] * d;
        mat[7] += mat[11] * d;
        d = mat[14] * di;
        mat[12] += mat[8] * d;
        mat[13] += mat[9] * d;
        mat[15] += mat[11] * d;
        di = mat[15];
        s *= di;
        mat[15] = d = 1.0f / di;
        mat[12] *= d;
        mat[13] *= d;
        mat[14] *= d;
        d = -d;
        mat[3] *= d;
        mat[7] *= d;
        mat[11] *= d;
        d = mat[3] * di;
        mat[0] += mat[12] * d;
        mat[1] += mat[13] * d;
        mat[2] += mat[14] * d;
        d = mat[7] * di;
        mat[4] += mat[12] * d;
        mat[5] += mat[13] * d;
        mat[6] += mat[14] * d;
        d = mat[11] * di;
        mat[8] += mat[12] * d;
        mat[9] += mat[13] * d;
        mat[10] += mat[14] * d;

        return ( s != 0.0f && !FLOAT_IS_NAN( s ) );
#else
        //	6*8+2*6 = 60 multiplications
        //		2*1 =  2 divisions
        Mat2 r0, r1, r2, r3;
        float a, det, invDet;
        float *mat = reinterpret_cast<float *>(this);

        // r0 = m0.inverse();
        det = mat[0*4+0] * mat[1*4+1] - mat[0*4+1] * mat[1*4+0];

        if ( math::fabs( det ) < MATRIX_INVERSE_EPSILON ) {
            return false;
        }

        invDet = 1.0f / det;

        r0[0][0] =   mat[1*4+1] * invDet;
        r0[0][1] = - mat[0*4+1] * invDet;
        r0[1][0] = - mat[1*4+0] * invDet;
        r0[1][1] =   mat[0*4+0] * invDet;

        // r1 = r0 * m1;
        r1[0][0] = r0[0][0] * mat[0*4+2] + r0[0][1] * mat[1*4+2];
        r1[0][1] = r0[0][0] * mat[0*4+3] + r0[0][1] * mat[1*4+3];
        r1[1][0] = r0[1][0] * mat[0*4+2] + r0[1][1] * mat[1*4+2];
        r1[1][1] = r0[1][0] * mat[0*4+3] + r0[1][1] * mat[1*4+3];

        // r2 = m2 * r1;
        r2[0][0] = mat[2*4+0] * r1[0][0] + mat[2*4+1] * r1[1][0];
        r2[0][1] = mat[2*4+0] * r1[0][1] + mat[2*4+1] * r1[1][1];
        r2[1][0] = mat[3*4+0] * r1[0][0] + mat[3*4+1] * r1[1][0];
        r2[1][1] = mat[3*4+0] * r1[0][1] + mat[3*4+1] * r1[1][1];

        // r3 = r2 - m3;
        r3[0][0] = r2[0][0] - mat[2*4+2];
        r3[0][1] = r2[0][1] - mat[2*4+3];
        r3[1][0] = r2[1][0] - mat[3*4+2];
        r3[1][1] = r2[1][1] - mat[3*4+3];

        // r3.inverse_self();
        det = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0];

        if ( math::fabs( det ) < MATRIX_INVERSE_EPSILON ) {
            return false;
        }

        invDet = 1.0f / det;

        a = r3[0][0];
        r3[0][0] =   r3[1][1] * invDet;
        r3[0][1] = - r3[0][1] * invDet;
        r3[1][0] = - r3[1][0] * invDet;
        r3[1][1] =   a * invDet;

        // r2 = m2 * r0;
        r2[0][0] = mat[2*4+0] * r0[0][0] + mat[2*4+1] * r0[1][0];
        r2[0][1] = mat[2*4+0] * r0[0][1] + mat[2*4+1] * r0[1][1];
        r2[1][0] = mat[3*4+0] * r0[0][0] + mat[3*4+1] * r0[1][0];
        r2[1][1] = mat[3*4+0] * r0[0][1] + mat[3*4+1] * r0[1][1];

        // m2 = r3 * r2;
        mat[2*4+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0];
        mat[2*4+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1];
        mat[3*4+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0];
        mat[3*4+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1];

        // m0 = r0 - r1 * m2;
        mat[0*4+0] = r0[0][0] - r1[0][0] * mat[2*4+0] - r1[0][1] * mat[3*4+0];
        mat[0*4+1] = r0[0][1] - r1[0][0] * mat[2*4+1] - r1[0][1] * mat[3*4+1];
        mat[1*4+0] = r0[1][0] - r1[1][0] * mat[2*4+0] - r1[1][1] * mat[3*4+0];
        mat[1*4+1] = r0[1][1] - r1[1][0] * mat[2*4+1] - r1[1][1] * mat[3*4+1];

        // m1 = r1 * r3;
        mat[0*4+2] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0];
        mat[0*4+3] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1];
        mat[1*4+2] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0];
        mat[1*4+3] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1];

        // m3 = -r3;
        mat[2*4+2] = -r3[0][0];
        mat[2*4+3] = -r3[0][1];
        mat[3*4+2] = -r3[1][0];
        mat[3*4+3] = -r3[1][1];

        return true;
#endif
    }

    /*
    =============
    Mat4::to_string
    =============
    */
    const char *Mat4::to_string( int precision ) const {
        return float_array_to_string_ANSI( to_float_ptr(), get_dimension(), precision );
    }


    /*
    ============
    Mat3::to_Angles_maya

    See Angles::to_Mat3_Maya for specification of rotation matrices and order

    ============
    */
    Angles Mat3::to_Angles_maya( void ) const {
        Angles	angles;
        double		theta;
        double		cp;
        float		sp;

        sp = mat[ 0 ][ 2 ];

        // cap off our sin value so that we don't get any NANs
        if ( sp > 1.0f ) {
            sp = 1.0f;
        } else if ( sp < -1.0f ) {
            sp = -1.0f;
        }

        theta = -math::asin( sp );
        cp = math::cos( theta );

        if ( cp > 8192.0f * math::FLOAT_EPSILON ) {
            angles.yaw		= math::RAD2DEG( theta );
            angles.roll		= math::RAD2DEG( math::atan( mat[ 0 ][ 1 ], mat[ 0 ][ 0 ] ) );
            angles.pitch	= math::RAD2DEG( math::atan( mat[ 1 ][ 2 ], mat[ 2 ][ 2 ] ) );
        } else {
            angles.yaw		= math::RAD2DEG( theta );
            angles.roll		= math::RAD2DEG( math::atan( mat[ 1 ][ 0 ], mat[ 1 ][ 1 ] ) );
            angles.pitch	= 0;
        }
        return angles;
    }


}   // ::inf
